function [ctrl, aux] = GuidanceSLoop(t, statet, statef, ctrlp, auxp, alphar)
global Vs d2r m S R0 dAziUp dAziDown Vdown ts g0
rad  = statet(1);
lon = statet(2);
lat = statet(3);
V = statet(4);
fpa = statet(5);
azi = statet(6);
lonf = statef(2);
latf = statef(3);
alphap = ctrlp(1);
bankp = ctrlp(2);
signOfBankp = ctrlp(3);
kh = -0.04 + 2e-5*V*Vs;
if V > Vdown
    kh = 0.05;
elseif V > 3000/Vs
    kh = (0.05 - 0.001)*(V - 3000/Vs)/(Vdown - 3000/Vs) + 0.001;
else
    kh = 0.001;
end
%% magnitude of bank
StogoExp = acos(sin(lat)*sin(latf) + cos(lat)*cos(latf)*cos(lonf-lon));
tgoExp = 1500/ts - t;
% 牛顿迭代法1 迭代初值 bank0 = bankbasep; bank1 = bankbasep + d2r;
[bankbase, F, iter] = newtonIteration1(@(bank)PredIntegral1(statet, bank,...
    StogoExp, tgoExp), auxp(2), auxp(2) + d2r, 5, 1e-4);
G = 0;
absBankf = auxp(3);
% QEGC补偿
alt = (rad-1)*R0;
[~, sonic, ~, rho] = atmoscoesa(alt);
Ma = V*Vs/sonic;
[CL, ~] = getCLCD(alphar, Ma);
[~, CD] = getCLCD(alphap, Ma);
q = 0.5*rho*(V*Vs)^2;
L = q*S*CL/m;
D = q*S*CD/m;
n = sqrt(L^2 + D^2)/g0;
dQ = 6E-8*sqrt(rho)*(V*Vs)^3.15;
dhc = - D * 2 * 7110 / (V*Vs);
dh = (V*Vs) * sin(fpa);
alphac = alphar;
bankc = acos((L*cos(bankbase) + kh * (dhc - dh))/L);
if lat >= latf || lon >= lonf
    bankbase = auxp(2);
    bankc = bankp;
end
%% sign of bank
aziLos = atan(sin(lonf-lon)/(cos(lat)*tan(latf) - sin(lat)*cos(lonf-lon)));
dAzi = azi - aziLos;
if dAzi > dAziUp
    signOfBank = -1;
elseif dAzi > dAziDown
    signOfBank = signOfBankp;
else
    signOfBank = 1;
end
ctrl(1) = alphac;
ctrl(2) = bankc;
ctrl(3) = signOfBank;
% stogo bankbase bankf dAzi iter F G 
aux(1) = StogoExp;
aux(2) = bankbase;
aux(3) = absBankf;
aux(4) = dAzi;
aux(5) = iter;
aux(6) = F;
aux(7) = G;
aux(8) = dQ;
aux(9) = n;
aux(10) = q;
end